﻿#include "FocusScan.h"
#include <QDebug>
#include <QElapsedTimer>
//#include "libswscale/swscale.h"
//#include "libavutil/pixfmt.h"

namespace Qly {

FocusScan::FocusScan(QObject *parent)
    :Qly::AutoFocusInterface(parent)
{
    m_pWatchDog = new QTimer(this);
    m_pWatchDog->setSingleShot(true);
    connect(m_pWatchDog, &QTimer::timeout, this, &FocusScan::watchdogTimeout);
}

void FocusScan::stop()
{
    m_state = ON_IDLE;
    m_pWatchDog->stop();
    m_pStageControl->stopZ();
}

void FocusScan::newFocusValue(QImage &image, double score)
{
    switch (m_state) {
    case ON_MOVE_DOWN:
        if(score > m_best_score)
        {
            m_best_score = score;
            m_best_image = image;
        }
        break;
    default:
        break;
    }
}

void FocusScan::zMoveFinished(int32_t stopcode)
{
    switch (m_state)
    {
    case ON_MOVE_UP: //向上运动已经完成了，开始向下运动
        //stopWatchDog();
        m_state = ON_MOVE_DOWN;
        m_best_score = 0.0;
        m_pStageControl->zMoveToPos(m_target_down_mm, m_speed_mmps);
        //startWatchDog();
        break;
    case ON_MOVE_DOWN:
        //stopWatchDog();
        m_state = ON_GO_BACK;
        m_pStageControl->zMoveToPos(m_z_mm, 2.0); // 回到原始位置
        //startWatchDog();
        break;
    case ON_GO_BACK:
        m_pWatchDog->stop();
        m_state = ON_IDLE;
        emit autoFocusFinished(true);
        break;
    default: //这时候什么都不做
        break;
    }
}

void FocusScan::watchdogTimeout()
{
    m_pStageControl->stopZ();
    m_pWatchDog->stop();
    m_state = ON_IDLE;
    emit autoFocusFinished(false);
}
void FocusScan::startWatchDog(int ms)
{
    if(ms <= 0)
    {
        m_pWatchDog->start(5000);
        return;
    }
    m_pWatchDog->start(ms);
}

void FocusScan::startAutoFocus()
{
    qDebug() << "FocusScan::startAutoFocus()";
    m_best_score = 0.0;
    if(m_high_mm == 0.0 && m_low_mm == 0.0)
    {
        //这时不用做自动对焦。当前图像就是最佳图像
        m_state = ON_IDLE;
        emit autoFocusFinished(false);
        return;
    }

    qDebug() << "H1";
    m_z_mm = m_pStageControl->zTheoryPos_mm();
    m_target_up_mm = m_z_mm + m_high_mm;
    m_target_down_mm = m_z_mm + m_low_mm;

    qDebug() << "m_target_up_mm = " << m_target_up_mm << ", m_target_down_mm = " << m_target_down_mm;
    //m_pCamera->startGrabbing(); // 必须保证相机是采图模式
    m_state = ON_MOVE_UP; // 先向上运动
    m_pStageControl->zMoveToPos(m_target_up_mm, 2.0);

    qDebug() << "Here";
    startWatchDog();
}

double FocusScan::bestImage(QImage &image)
{
    image = m_best_image;
    return m_best_score;
}

}
